#include "ros/ros.h"
#include <custom_msgs/AddTwoInts.h>

bool add(custom_msgs::AddTwoInts::Request &req,
         custom_msgs::AddTwoInts::Response &res)
{
    res.sum = req.a + req.b;
    ROS_INFO("Request: a=%ld, b=%ld", req.a, req.b);
    ROS_INFO("Response: sum=%ld", res.sum);
    return true;
}

int main(int argc, char **argv)
{
    // 初始化 ROS 节点
    ros::init(argc, argv, "add_two_ints_server");
    ros::NodeHandle nh;

    // 检查 ROS Master 是否可用
    if (!ros::master::check())
    {
        ROS_FATAL("Failed to connect to ROS Master! Is roscore running?");
        return 1; // 非正常退出
    }

    // 尝试连接 Master，如果失败则等待
    while (!ros::master::check())
    {
        ROS_WARN("Waiting for ROS Master (roscore) to start...");
        ros::Duration(1.0).sleep(); // 每秒检查一次
    }

    // 注册服务
    ros::ServiceServer service = nh.advertiseService("add_two_ints", add);
    ROS_INFO("Service server ready. Waiting for requests...");

    // 进入事件循环
    ros::spin();

    return 0;
}